#ifndef VMC_LANE_FULL_MATCHING_GEOMETRY
#define VMC_LANE_FULL_MATCHING_GEOMETRY

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <vector>

class geometry
{
public:
	static void Resample(std::vector<Eigen::Vector3d> &polyline,
						 double interval);

	static double GetLength(const std::vector<Eigen::Vector3d> &polyline);

	static double DistancePtToLine3D(Eigen::Vector3d pt,
									 Eigen::Vector3d ptStart,
									 Eigen::Vector3d ptEnd);

	static double DistancePtToLine2D(Eigen::Vector2d pt,
									 Eigen::Vector2d ptStart,
									 Eigen::Vector2d ptEnd);

	static double DistancePtToLineSegment3D(Eigen::Vector3d pt,
											Eigen::Vector3d ptStart,
											Eigen::Vector3d ptEnd);

	static double DistancePtToLineSegment2D(Eigen::Vector2d pt,
											Eigen::Vector2d ptStart,
											Eigen::Vector2d ptEnd);
	
	static double DistancePolylineToPolyline2D(const std::vector<Eigen::Vector3d>& lhs,
										  const std::vector<Eigen::Vector3d>& rhs);

	static bool GetProjectPointToPolyline(const Eigen::Vector3d &pnt,
										  const std::vector<Eigen::Vector3d> &lineString,
										  Eigen::Vector3d &pntPrj,
										  int &nSegIndex);

	static bool IsProject(const Eigen::Vector3d &pnt,
						  const Eigen::Vector3d &ptStart,
						  const Eigen::Vector3d &ptEnd);

	static bool IsOverlap(const Eigen::Vector3d &s1,
						  const Eigen::Vector3d &e1,
						  const Eigen::Vector3d &s2,
						  const Eigen::Vector3d &e2);

	static bool IsOverlap(const std::vector<Eigen::Vector3d> &line1,
						  const std::vector<Eigen::Vector3d> &line2);

	static double GetNearestDisPtToPolyLine(const Eigen::Vector3d &pnt,
											const std::vector<Eigen::Vector3d> &lineString,
											Eigen::Vector3d &pntHit,
											int &nSegIndex);

	static bool IsPntLeftSideToLine3D(const Eigen::Vector3d &pnt,
									  const Eigen::Vector3d &ptStart,
									  const Eigen::Vector3d &ptEnd);

	static bool IsPntLeftSideToPolyLine3D(const Eigen::Vector3d &pnt,
										  const std::vector<Eigen::Vector3d> &lineString);

	static double Maximum(double a, double b);

	static double Minimum(double a, double b);

	static bool GetHeadTailDistance(const std::vector<Eigen::Vector3d> &lhs,
									const std::vector<Eigen::Vector3d> &rhs,
									double &head_dis,
									double &tail_dis);

	static double GetMinDistance(const std::vector<Eigen::Vector3d> &lhs,
								 const std::vector<Eigen::Vector3d> &rhs);

	static double GetAngleBetween(const std::vector<Eigen::Vector3d> &lhs,
								  const std::vector<Eigen::Vector3d> &rhs);

	static double NormalDistribution(double x,
									 double u,
									 double sigma);

	static Eigen::Matrix3d RotationAroundX(const double &angle);
	static Eigen::Matrix3d RotationAroundY(const double &angle);
	static Eigen::Matrix3d RotationAroundZ(const double &angle);
	static double D2R(double degree);
	static double R2D(double radians);
};

#endif